/*!
 * @file CameraActuator.cpp
 * @author Rocco Martino
 */
/***************************************************************************
 *   Copyright (C) 2014 by Rocco Martino                                   *
 *   martinorocco@gmail.com                                                *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU Lesser General Public License as        *
 *   published by the Free Software Foundation; either version 2.1 of the  *
 *   License, or (at your option) any later version.                       *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this program; if not, write to the                 *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

/* ======================================================================= */
/* ....................................................................... */
#include <ooGame/CameraActuator>

#include <osgODE/Notify>
#include <osgODE/RigidBody>
#include <osgODE/World>
/* ....................................................................... */
/* ======================================================================= */




using namespace ooGame ;




/* ======================================================================= */
/* ....................................................................... */
CameraActuator::CameraActuator(void):
    m_dirty                     ( true ),
    m_distance_min              ( 0.0 ),
    m_distance_max              ( FLT_MAX ),
    m_height                    ( 0.0 ),
    m_axis                      ( osg::X_AXIS ),
    m_up_world                  ( osg::Z_AXIS )
{
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
CameraActuator::CameraActuator(const CameraActuator& other, const osg::CopyOp& copyop):
    Actuator                    ( other, copyop ),
    m_dirty                     ( other.m_dirty ),
    m_track_to_by_name_internal ( other.m_track_to_by_name_internal ),
    m_track_to                  ( other.m_track_to.get() ),
    m_linear_pid                ( osg::clone(other.m_linear_pid.get(), copyop) ),
    m_angular_pid               ( osg::clone(other.m_angular_pid.get(), copyop) ),
    m_distance_min              ( other.m_distance_min ),
    m_distance_max              ( other.m_distance_max ),
    m_height                    ( other.m_height ),
    m_axis                      ( other.m_axis ),
    m_up_world                  ( other.m_up_world )
{
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
CameraActuator::~CameraActuator(void)
{
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
void
CameraActuator::actuate( osgODE::ODEObject* object, Game* game )
{
    osgODE::RigidBody*  body = object->asRigidBody() ;


    PS_ASSERT1( body != NULL ) ;
    PS_ASSERT1( body->getWorld() != NULL ) ;
    PS_ASSERT1( body->getWorld()->getFrontEventsBuffer() != NULL ) ;

    (void) game ;





    if( m_dirty ) {

        if( m_track_to_by_name_internal != "" ) {

            osgODE::World*      world = body->getWorld() ;

            osgODE::ODEObject*  obj = world->getObjectByName( m_track_to_by_name_internal ) ;


            if( obj ) {

                m_track_to = obj->asRigidBody() ;

            }
        }

        m_dirty = false ;
    }





    PS_ASSERT1( m_track_to.valid() ) ;



    osgODE::World*  world = body->getWorld() ;




    // Linear
    {
        const osg::Vec3 axis_world = m_track_to->getQuaternion() * m_axis ;

        osg::Vec3   tmp = m_track_to->getPosition() - body->getPosition() ;

        ooReal      cur_distance = tmp.normalize() ;

        const osg::Vec3 position = m_track_to->getPosition() - axis_world * osg::clampTo(cur_distance, m_distance_min, m_distance_max) + m_up_world * m_height ;

        osg::Vec3   direction =  position - body->getPosition() ;

        ooReal  linear_error = direction.normalize() ;

        osg::Vec3   force ;

        if( m_linear_pid ) {
            force = direction * m_linear_pid->solve( linear_error, world->getCurrentStepSize() ) ;
        } else {
            force = direction * linear_error ;
        }

        body->addForce( force ) ;
    }



    // Angular
    {
        osg::Quat   quat = body->getQuaternion() ;


        osg::Vec3   front = m_track_to->getPosition() - body->getPosition() ;

        front.normalize() ;



        osg::Vec3   up = front ^ m_up_world ;

        up = up ^ front ;
        up.normalize() ;




        osg::Quat   rot1 ;

        rot1.makeRotate( quat * -osg::Z_AXIS, front ) ;



        osg::Quat   rot2 ;

        rot2.makeRotate( quat * osg::Y_AXIS, up ) ;


        osg::Quat   qerr = rot1 * rot2 ;

        osg::Vec3   axis( qerr.x(), qerr.y(), qerr.z() ) ;

        float   angle = axis.normalize() ;

        if( osg::absolute( qerr.w() ) > 0.5 ) {
            angle = asin( angle ) * 2.0 ;
        } else {
            angle = acos( qerr.w() ) * 2.0 ;
        }

        if( angle == 0.0 ) {
            axis = osg::Z_AXIS ;
        }





        if( m_angular_pid.valid() ) {

            ooReal  mag = m_angular_pid->solve( angle, world->getCurrentStepSize() ) ;
            body->addTorque( axis * mag ) ;

        } else {
            body->addTorque( axis * angle ) ;
        }
    }







    osgODE::Events::EventList&  events = world->getFrontEventsBuffer()->getEventList() ;

    for( unsigned int i=0; i<events.size(); i++ ) {

        osg::View*  view = events[i].second.get() ;

        if( view ) {
            view->getCamera()->setViewMatrix( osg::Matrix::inverse( body->getMatrix() ) ) ;
        }
    }
}
/* ....................................................................... */
/* ======================================================================= */
